
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       CompassCalibrator.h
  * @author     baiyang
  * @date       2021-12-11
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <rtthread.h>

#include <common/gp_config.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define COMPASS_CAL_NUM_SPHERE_PARAMS       4
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS    9
#define COMPASS_CAL_NUM_SAMPLES             300     // number of samples required before fitting begins

#define COMPASS_MAX_SCALE_FACTOR 1.5
#define COMPASS_MIN_SCALE_FACTOR (1.0/COMPASS_MAX_SCALE_FACTOR)
/*----------------------------------typedef-----------------------------------*/
// compass calibration states
enum CompassCalStatus {
    COMPASS_CAL_NOT_STARTED = 0,
    COMPASS_CAL_WAITING_TO_START = 1,
    COMPASS_CAL_RUNNING_STEP_ONE = 2,
    COMPASS_CAL_RUNNING_STEP_TWO = 3,
    COMPASS_CAL_SUCCESS = 4,
    COMPASS_CAL_FAILED = 5,
    COMPASS_CAL_BAD_ORIENTATION = 6,
    COMPASS_CAL_BAD_RADIUS = 7,
};

// get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
typedef uint8_t completion_mask_t[10];

// Structure accessed for cal status update via mavlink 
struct CompassCalState {
    enum CompassCalStatus status;
    uint8_t attempt;
    float completion_pct;
    completion_mask_t completion_mask;
};

// Structure accessed after calibration is finished/failed
struct CompassCalReport {
    enum CompassCalStatus status;
    float fitness;
    Vector3f_t ofs;
    Vector3f_t diag;
    Vector3f_t offdiag;
    float orientation_confidence;
    enum RotationEnum original_orientation;
    enum RotationEnum orientation;
    float scale_factor;
    bool check_orientation;
};

// Structure setup to set calibration run settings
struct CompassCalSettings {
    float tolerance;
    bool check_orientation;
    enum RotationEnum orientation;
    enum RotationEnum orig_orientation;
    bool is_external;
    bool fix_orientation;
    uint16_t offset_max;
    uint8_t attempt;
    bool retry;
    float delay_start_sec;
    uint32_t start_time_ms;
    uint8_t compass_idx;
    bool always_45_deg;
};

/** @ 
  * @brief  
  */
typedef struct {
    float radius;       // magnetic field strength calculated from samples
    Vector3f_t offset;    // offsets
    Vector3f_t diag;      // diagonal scaling
    Vector3f_t offdiag;   // off diagonal scaling
    float scale_factor; // scaling factor to compensate for radius error
} CompassCalParam;

/** @ 
  * @brief  
  */
typedef struct {
    int8_t roll;
    int8_t pitch;
    int8_t yaw;
} CompassCalAttSample;

/** @ 
  * @brief  
  */
typedef struct {
    CompassCalAttSample att;

    int16_t x;
    int16_t y;
    int16_t z;
} CompassSample;

/** @ 
  * @brief  
  */
typedef struct {
    struct CompassCalState cal_state;
    struct CompassCalReport cal_report;
    struct CompassCalSettings cal_settings;

    uint8_t _compass_idx;                   // index of the compass providing data
    enum CompassCalStatus _status;          // current state of calibrator

    // values provided by caller
    float _delay_start_sec;                 // seconds to delay start of calibration (provided by caller)
    bool _retry;                            // true if calibration should be restarted on failured (provided by caller)
    float _tolerance;                       // worst acceptable RMS tolerance (aka fitness).  see set_tolerance(),default 5.0
    uint16_t _offset_max;                   // maximum acceptable offsets (provided by caller)

    // behavioral state
    uint32_t _start_time_ms;                // system time start() function was last called
    uint8_t _attempt;                       // number of attempts have been made to calibrate
    completion_mask_t _completion_mask;     // bitmask of directions in which we have samples
    CompassSample *_sample_buffer;          // buffer of sensor values
    uint16_t _samples_collected;            // number of samples in buffer
    uint16_t _samples_thinned;              // number of samples removed by the thin_samples() call (called before step 2 begins)

    // fit state
    CompassCalParam _params;                  // latest calibration outputs
    uint16_t _fit_step;                     // step during RUNNING_STEP_ONE/TWO which performs sphere fit and ellipsoid fit
    float _fitness;                         // fitness (mean squared residuals) of current parameters
    float _initial_fitness;                 // fitness before latest "fit" was attempted (used to determine if fit was an improvement)
    float _sphere_lambda;                   // sphere fit's lambda
    float _ellipsoid_lambda;                // ellipsoid fit's lambda

    // variables for orientation checking
    enum RotationEnum _orientation;             // latest detected orientation
    enum RotationEnum _orig_orientation;        // original orientation provided by caller
    enum RotationEnum _orientation_solution;    // latest solution
    bool _is_external;                      // true if compass is external (provided by caller)
    bool _check_orientation;                // true if orientation should be automatically checked
    bool _fix_orientation;                  // true if orientation should be fixed if necessary
    bool _always_45_deg;                    // true if orentation should considder 45deg with equal tolerance
    float _orientation_confidence;          // measure of confidence in automatic orientation detection
    CompassSample _last_sample;

    enum CompassCalStatus _requested_status;
    bool   _status_set_requested;

    bool _new_sample;

    // Semaphore for state related intermediate structures
    struct rt_mutex state_sem;

    // Semaphore for intermediate structure for point sample collection
    struct rt_mutex sample_sem;
} CompassCalibrator;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void compasscal_ctor(CompassCalibrator *cal, int8_t instance);
void compasscal_stop(CompassCalibrator *cal);
void compasscal_set_orientation(CompassCalibrator *cal, enum RotationEnum orientation, bool is_external, bool fix_orientation, bool always_45_deg);
void compasscal_start(CompassCalibrator *cal, bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance);
void compasscal_new_sample(CompassCalibrator *cal, const Vector3f_t* sample);
bool compasscal_failed(CompassCalibrator *cal);
bool compasscal_running(CompassCalibrator *cal);
void compasscal_update(CompassCalibrator *cal);
const struct CompassCalReport compasscal_get_report(CompassCalibrator *cal);
const struct CompassCalState compasscal_get_state(CompassCalibrator *cal);
enum RotationEnum compasscal_auto_rotation_index(uint8_t n);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



